#!/usr/bin/env python
import rospy
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from prometheus_msgs.msg import DetectionInfo
from sensor_msgs.msg import Image


detect_info = DetectionInfo()
detect_info.object_name = "fire_point"

def image_cb(msg, bridge, detect_pub, params):
    try:
        cv_img = bridge.imgmsg_to_cv2(msg, "bgr8")
        shape = cv_img.shape
        median = cv.medianBlur(cv_img, 5)
        hsv = cv.cvtColor(median, cv.COLOR_BGR2HSV)

        thresh = cv.inRange(hsv, np.array([0, 137, 123]), np.array([180, 255, 255]))
        
        if cv.__version__.startswith('3'):
            _, contours, hierarchy = cv.findContours(thresh, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
        elif cv.__version__.startswith('4'):
            contours, hierarchy = cv.findContours(thresh, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
        else:
            raise NotImplementedError("Unknown version " + cv.__version__)
        
        if len(contours) > 0:
            areas = [cv.contourArea(contour) for contour in contours]
            max_index = areas.index(max(areas))
            max_area = max(areas)
            (x, y), radius = cv.minEnclosingCircle(contours[max_index])

            factor = max_area / (radius * radius * 3.1415)

            if radius > params['min_radius'] and factor > 0.5:
                rospy.loginfo('detected!!!!!')
                
                cv.drawContours(cv_img, contours, max_index, (0, 255, 255), 3)
            
                center = (int(x),int(y))
                radius = int(radius)
                cv.circle(cv_img,center,radius,(0,255,0),2)

                position0 = round((center[0] - (shape[1] / 2)) / (shape[1] / 2), 3)
                position1 = round((center[1] - (shape[0] / 2)) / (shape[0] / 2), 3)
                detect_info.position[0] = position0
                detect_info.position[1] = position1
                detect_info.detected = True

                cv.putText(cv_img, "center:[{1},{0}], r:[{2}]".format(position0, position1, radius), center, 1, 1, (0, 255, 0), 1)
            else:
                # print(radius, factor)
                detect_info.detected = False
        else:
            detect_info.detected = False
            # print('no contours, check hsv')
        
        detect_pub.publish(detect_info)
        # cv.imshow('frame', cv_img)
        # cv.imshow('thresh', thresh)
        # key = cv.waitKey(1)

    except CvBridgeError as e:
        rospy.logerr(e)

if __name__ == '__main__':
    rospy.init_node('detect_fire_point')
    rate = rospy.Rate(20)
    bridge = CvBridge()

    params = {}
    params['min_radius'] = rospy.get_param('~min_radius', 10.0)
    rospy.loginfo("param of min_radius is " + str(params['min_radius']))

    detect_pub = rospy.Publisher('/prometheus/object_detection/firepoint', DetectionInfo, queue_size=2)

    img_sub = rospy.Subscriber('/usb_cam/image_raw', Image, lambda msg: image_cb(msg, bridge, detect_pub, params))

    rospy.spin()
   
